#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
class CameraTFPublisher : public rclcpp::Node
{
public:
    CameraTFPublisher()
        : Node("camera_tf2_broadcaster")
    {
        RCLCPP_INFO(this->get_logger(), "Camera TF2 broadcaster started.");

        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        left_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/camera/left/image_raw", rclcpp::SensorDataQoS(),
            std::bind(&CameraTFPublisher::left_callback, this, std::placeholders::_1));

        right_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/camera/right/image_raw", rclcpp::SensorDataQoS(),
            std::bind(&CameraTFPublisher::right_callback, this, std::placeholders::_1));

        // ori_cam_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
        //     "/usb_cam_raw/image_raw", rclcpp::SensorDataQoS(),
        //     std::bind(&CameraTFPublisher::ori_callback, this, std::placeholders::_1));

        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&CameraTFPublisher::broadcast_transforms, this));
    }

private:
    void left_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        left_stamp_ = msg->header.stamp;
    }

    void right_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        right_stamp_ = msg->header.stamp;
    }

    // void ori_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    // {
    //     ori_stamp_ = msg->header.stamp;
    // }

    void broadcast_transforms()
    {
        if (left_stamp_.nanoseconds() == 0 || right_stamp_.nanoseconds() == 0)
            return;

        rclcpp::Time now = this->get_clock()->now();

        // world → 左相机
        geometry_msgs::msg::TransformStamped left_tf;
        left_tf.header.stamp = now;
        left_tf.header.frame_id = "world";
        left_tf.child_frame_id = "l_camera_optical_frame";
        left_tf.transform.translation.x = 0.0;
        left_tf.transform.translation.y = 0.0;
        left_tf.transform.translation.z = 0.0;
        left_tf.transform.rotation.x = 0.0;
        left_tf.transform.rotation.y = 0.0;
        left_tf.transform.rotation.z = 0.0;
        left_tf.transform.rotation.w = 1.0;
        tf_broadcaster_->sendTransform(left_tf);

        // l_camera_optical_frame → r_camera_optical_frame
        geometry_msgs::msg::TransformStamped right_tf;
        right_tf.header.stamp = now;
        right_tf.header.frame_id = "l_camera_optical_frame";  // 以左相机为参考系
        right_tf.child_frame_id = "r_camera_optical_frame";
        right_tf.transform.translation.x = 0.065;  // 基线6.5cm
        right_tf.transform.translation.y = 0.0;
        right_tf.transform.translation.z = 0.0;
        right_tf.transform.rotation.x = 0.0;
        right_tf.transform.rotation.y = 0.0;
        right_tf.transform.rotation.z = 0.0;
        right_tf.transform.rotation.w = 1.0;
        tf_broadcaster_->sendTransform(right_tf);

    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr left_subscription_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr right_subscription_;
    // rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr ori_cam_subscription_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

    rclcpp::Time left_stamp_;
    rclcpp::Time right_stamp_;
    // rclcpp::Time ori_stamp_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CameraTFPublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

        // world → left_camera_link
        // geometry_msgs::msg::TransformStamped left_tf;
        // left_tf.header.stamp = now;
        // left_tf.header.frame_id = "world";
        // left_tf.child_frame_id = "l_camera_optical_frame";
        // left_tf.transform.translation.x = 0.0;
        // left_tf.transform.translation.y = 0.0;
        // left_tf.transform.translation.z = 0.0;
        // left_tf.transform.rotation.x = 0.0;
        // left_tf.transform.rotation.y = 0.0;
        // left_tf.transform.rotation.z = 0.0;
        // left_tf.transform.rotation.w = 1.0;
        // tf_broadcaster_->sendTransform(left_tf);

        // // left_camera_link → right_camera_link
        // geometry_msgs::msg::TransformStamped right_tf;
        // right_tf.header.stamp = now;
        // right_tf.header.frame_id = "world";
        // right_tf.child_frame_id = "r_camera_optical_frame";
        // right_tf.transform.translation.x = 0.1;
        // right_tf.transform.translation.y = 0.0;
        // right_tf.transform.translation.z = 0.0;
        // right_tf.transform.rotation.x = 0.0;
        // right_tf.transform.rotation.y = 0.0;
        // right_tf.transform.rotation.z = 0.0;
        // right_tf.transform.rotation.w = 1.0;
        // tf_broadcaster_->sendTransform(right_tf);

        // // left_camera_link → right_camera_link
        // geometry_msgs::msg::TransformStamped ori_cam_tf;
        // ori_cam_tf.header.stamp = now;
        // ori_cam_tf.header.frame_id = "world";
        // ori_cam_tf.child_frame_id = "camera_ori";
        // ori_cam_tf.transform.translation.x = 0.5;
        // ori_cam_tf.transform.translation.y = 0.0;
        // ori_cam_tf.transform.translation.z = 0.0;
        // ori_cam_tf.transform.rotation.x = 0.0;
        // ori_cam_tf.transform.rotation.y = 0.0;
        // ori_cam_tf.transform.rotation.z = 0.0;
        // ori_cam_tf.transform.rotation.w = 1.0;
        // tf_broadcaster_->sendTransform(ori_cam_tf);